#include <stdlib.h>

#include "../image.h"
#include "../io/imageio.h"
#include "../filters/imfilter.h"
#include "../../util/macros.h"
#include "../../util/misc.h"
#include "../../matrix/dsp/dsp.h"  
#include "../../matrix/filters/filters.h"

static void _get_harris_derivatives(Matrix * Gray, Matrix ** Dx, Matrix ** Dy, Matrix ** Dxy) {

	double * gray = NULL;
	double * dx = NULL;
	double * dy = NULL;
	double * dxy = NULL;

	Matrix * Dxi = NULL;
	Matrix * Dyi = NULL;
	double * dxi = NULL;
	double * dyi = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int imsize;

	int d;

	double sumx;
	double sumy;

	/* ====================================================================== */
	{
		_verify(Gray);
		_verify(Dx && Dy && Dxy);
	}
	/* ====================================================================== */
	{
		gray = Gray->data;
		rows = Gray->rows;
		columns = Gray->columns;
		imsize = Gray->size;
	}
	/* ====================================================================== */
	{
		if (*Dx == NULL) (*Dx) = matrix_new(rows, columns);
		if (*Dy == NULL) (*Dy) = matrix_new(rows, columns);
		if (*Dxy == NULL) (*Dxy) = matrix_new(rows, columns);

		dx = (*Dx)->data;
		dy = (*Dy)->data;
		dxy = (*Dxy)->data;

		Dxi = matrix_new(rows, columns);
		Dyi = matrix_new(rows, columns);
		dxi = Dxi->data;
		dyi = Dyi->data;
	}

	/* Step 01: Convolve with MaskCol ======================================= */
	{
		Matrix MaskDx;
		Matrix MaskDy;
		double * maskx = NULL;
		double * masky = NULL;

		MaskDx = matrix_get_filter("farid_simoncelli", 5, 'x', 1);
		MaskDy = matrix_get_filter("farid_simoncelli", 5, 'y', 1);
		maskx = MaskDx.data;
		masky = MaskDy.data;

		int mrows = MaskDx.rows;
		int mcolumns = MaskDx.columns;

		int rf;
		int mrc = (mrows - 1) / 2;
		int _2row_minus_2 = (rows << 1) - 2;
		int msize_minus_1 = (mrows * mcolumns) - 1;

		int * r_shift = create_int_sequence(rows, 0, columns);

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;
				sumx = 0;
				sumy = 0;

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					//d = rf * columns + c;
					d = r_shift[rf] + c;

					// reflect mask index
					mi = msize_minus_1 - mr;

					sumx += gray[d] * maskx[mi];
					sumy += gray[d] * masky[mi];

				}
				dxi[i] = sumx;
				dyi[i] = sumy;
			}
		}// </editor-fold>

		free(r_shift);

		//image_save_matrix_as_image(Dxi, "images/results/Dxi.pgm");
		//image_save_matrix_as_image(Dyi, "images/results/Dyi.pgm");


	}

	/* Step 02: Convolve with MaskRow ======================================= */
	{
		Matrix MaskDx;
		Matrix MaskDy;
		double * maskx = NULL;
		double * masky = NULL;

		MaskDx = matrix_get_filter("farid_simoncelli", 5, 'x', 2);
		MaskDy = matrix_get_filter("farid_simoncelli", 5, 'y', 2);
		maskx = MaskDx.data;
		masky = MaskDy.data;

		int mrows = MaskDx.rows;
		int mcolumns = MaskDx.columns;

		int cf;
		int mcc = (mcolumns - 1) / 2;
		int _2columns_minus_2 = columns << 1;
		int msize_minus_1 = (mrows * mcolumns) - 1;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {

			for (c = 0; c < columns; c++) {

				i = _r + c;
				sumx = 0;
				sumy = 0;


				for (mc = 0; mc < mcolumns; mc++) {

					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					//d = r * columns + cf;
					d = _r + cf;

					mi = _mr + mc;
					// reflect mask index
					mi = msize_minus_1 - mc;

					sumx += dxi[d] * maskx[mi];
					sumy += dyi[d] * masky[mi];
				}
				dx[i] = sumx * sumx;
				dy[i] = sumy * sumy;
				dxy[i] = sumx * sumy;
			}
		}// </editor-fold>

		//image_save_matrix_as_image(*Dy, "images/results/Dy.pgm");
		//image_save_matrix_as_image(*Dx, "images/results/Dx.pgm");
		//image_save_matrix_as_image(*Dxy, "images/results/Dxy.pgm");
	}

	matrix_delete(Dxi);
	matrix_delete(Dyi);
}

void image_features_harris(Matrix * Gray, Matrix ** Harris) {

	double * harris;

	Matrix * Dx = NULL;
	Matrix * Dy = NULL;
	Matrix * Dxy = NULL;

	double * dx;
	double * dy;
	double * dxy;

	Matrix * Dxc = NULL;
	Matrix * Dyc = NULL;
	Matrix * Dxyc = NULL;

	Matrix * Gauss1 = NULL;
	Matrix * Gauss2 = NULL;

	int rows;
	int columns;
	long imsize;

	int
	long i;

	_verify(Gray);
	_verify(Harris);

	{ // Initialization and memory allocation

		rows = Gray->rows;
		columns = Gray->columns;
		imsize = Gray->size;

	}
	{ // Step 01: Compute the gradients: Dx², Dy² and Dx*Dy

		_get_harris_derivatives(Gray, &Dx, &Dy, &Dxy);

	}
	{ // Step 03: Blurring with Gaussian filter ================================
		Gauss1 = filter_new_gaussian(3, 1, 1, 1, 0);
		Gauss2 = filter_new_gaussian(1, 3, 1, 1, 0);

		matrix_conv_2_pass(Dx, Gauss1, Gauss2, &Dxc);
		matrix_conv_2_pass(Dy, Gauss1, Gauss2, &Dyc);
		matrix_conv_2_pass(Dxy, Gauss1, Gauss2, &Dxyc);

		matrix_delete(Gauss1);
		matrix_delete(Gauss2);
	}
	{ // Step 04: Compute Harris response ======================================
		if (*Harris == NULL) (*Harris) = matrix_new(rows, columns);
		harris = (*Harris)->data;

		dx = Dxc->data;
		dy = Dyc->data;
		dxy = Dxyc->data;

		for (i = 0; i < imsize; i++) {
			// cim = (Ix2.*Iy2 - Ixy.^2)./(Ix2 + Iy2 + eps);
			harris[i] = (dx[i] * dy[i] - dxy[i] * dxy[i]) / (dx[i] + dy[i] + 2.22e-16);
		}
		//image_save_matrix_as_image(Harris, "images/results/Harris.pgm");

		matrix_delete(Dxc);
		matrix_delete(Dyc);
		matrix_delete(Dxyc);

		matrix_delete(Dx);
		matrix_delete(Dy);
		matrix_delete(Dxy);
	}
}

/*
Para amanha:
novo padrao de calibracao
identificar retangulos 
 */



